// find interpolated value of y 

#include   "inter_2d.h"

signed int16 inter_2d ( signed int16 x,
                        signed int16 x1, signed int16 x2,
                        signed int16 y1, signed int16 y2)
{
#if 0
   signed int32 temp;
   signed int16 y;
    temp = ((signed int32)(y2-y1))*(x-x1);
    y = y1+(signed int16)(temp/(x2-x1));
    return y;
#else
    return y1+(signed int16)((((signed int32)(y2-y1))*(x-x1))/(x2-x1));
#endif
}


signed int16 inter_3d ( signed int16 x, signed int16 y,
                        signed int16 x1, signed int16 x2,
                        signed int16 y1, signed int16 y2,
                        signed int16 z11, signed int16 z12,
                        signed int16 z21, signed int16 z22)
{
    signed int z1;
    signed int z2;
    z1 = inter_2d(x, x1, x2, z11, z12);
    z2 = inter_2d(x, x1, x2, z21, z22);
    return inter_2d(y, y1, y2, z1, z2);
}

#if 0
signed int16 fuzzy_control (signed int16 current, signed int16 set_point,
                            fuzzy_range_t *range)
{
    signed int16           out = 0;     //do nothing
    signed int16           error;
    static int16    last_err = 0;
    signed int16           d_err;
    char            x,y; 
    signed error = current - set_point;
    signed d_err = error - last_err;

    x = y = fuzzy_z;

    if (d_err > range->d_err[fuzzy_pb]) {
        x = fuzzy_pb;
    } else if (d_err > range->d_err[fuzzy_pm]) {
        x = fuzzy_pm;
    } else if (d_err >= range->d_err[fuzzy_ps]) {
        x = fuzzy_ps;
    } else if (d_err < range->d_err[fuzzy_nb]) {
        x = fuzzy_nb;
    } else if (d_err < range->d_err[fuzzy_nm]) {
        x = fuzzy_nm;
    } else if (d_err <= range->d_err[fuzzy_ns]) {
        x = fuzzy_ns;
    } 

    if (error > range->err[fuzzy_pb]) {
        y = fuzzy_pb;
    } else if (error > range->err[fuzzy_pm]) {
        y = fuzzy_pm;
    } else if (error >= range->err[fuzzy_ps]) {
        y = fuzzy_ps;
    } else if (error < range->err[fuzzy_nb]) {
        y = fuzzy_nb;
    } else if (error < range->err[fuzzy_nm]) {
        y = fuzzy_nm;
    } else if (error <= range->err[fuzzy_ns]) {
        y = fuzzy_ns;
    } 

    last_err = error; 
    return range->react[x][y];                 //incremental output
}
#else
signed int16 fuzzy_control (signed int16 current, signed int16 set_point,
                            fuzzy_range_t *range)
{
    signed int16           out = 0;     //do nothing
    signed int16           err;
    static int16    last_err = 0;
    signed int16           d_err;

    signed int16 xx1, xx2, yy1, yy2;
    
    err   = current - set_point;
    d_err = err - last_err;
    last_err = err; 

    if (d_err > range->d_err[fuzzy_pm]) {
        xx2 = fuzzy_pb;
        xx1 = fuzzy_pm;
    } else if (d_err >= range->d_err[fuzzy_ps]) {
        xx2 = fuzzy_pm;
        xx1 = fuzzy_ps;
    } else if (d_err < range->d_err[fuzzy_nm]) {
        xx2 = fuzzy_nm;
        xx1 = fuzzy_nb;
    } else if (d_err <= range->d_err[fuzzy_ns]) {
        xx2 = fuzzy_ns;
        xx1 = fuzzy_nm;
    } else if (err < range->d_err[fuzzy_z]) {
        xx2 = fuzzy_z;
        xx1 = fuzzy_ns;
    } else {
        xx2 = fuzzy_ps;
        xx1 = fuzzy_z;
    }

    if (err > range->err[fuzzy_pm]) {
        yy2 = fuzzy_pb;
        yy1 = fuzzy_pm;
    } else if (err >= range->err[fuzzy_ps]) {
        yy2 = fuzzy_pm;
        yy1 = fuzzy_ps;
    } else if (err < range->err[fuzzy_nm]) {
        yy2 = fuzzy_nm;
        yy1 = fuzzy_nb;
    } else if (err <= range->err[fuzzy_ns]) {
        yy2 = fuzzy_ns;
        yy1 = fuzzy_nm;
    } else if (err < range->err[fuzzy_z]) {
        yy2 = fuzzy_z;
        yy1 = fuzzy_ns;
    } else {
        yy2 = fuzzy_ps;
        yy1 = fuzzy_z;
    }
  
    return inter_3d(d_err, err,
                     range->d_err[xx1], range->d_err[xx2],
                     range->err[yy1], range->err[yy2],
                     range->react[xx1][yy1], range->react[xx1][yy2],
                     range->react[xx2][yy1], range->react[xx2][yy2]);

                 //incremental output
}

#endif
void fuzzy_init (fuzzy_range_t *range)
{
#define NB_R (200);
#define NM_R (100);
#define NS_R (50);
    
#define PB_R (-200);
#define PM_R (-100);
#define PS_R (-50);

    range->d_err[fuzzy_nb] = -600;
    range->d_err[fuzzy_nm] = -300;
    range->d_err[fuzzy_ns] = -150;
    range->d_err[fuzzy_z]  = 0;
    range->d_err[fuzzy_ps] = 150;
    range->d_err[fuzzy_pm] = 300;
    range->d_err[fuzzy_pb] = 600;
    
    range->err[fuzzy_nb] = -400;
    range->err[fuzzy_nm] = -200;
    range->err[fuzzy_ns] = -50;
    range->err[fuzzy_z]  = 0;
    range->err[fuzzy_ps] = 50;
    range->err[fuzzy_pm] = 200;
    range->err[fuzzy_pb] = 400;
                  
    range->react[fuzzy_nb][fuzzy_nb] = NB_R;
    range->react[fuzzy_nb][fuzzy_nm] = NB_R;
    range->react[fuzzy_nb][fuzzy_ns] = NB_R;
    range->react[fuzzy_nb][fuzzy_z]  = NM_R;
    range->react[fuzzy_nb][fuzzy_ps] = NM_R;
    range->react[fuzzy_nb][fuzzy_pm] = NS_R;
    range->react[fuzzy_nb][fuzzy_pb] = 0;

    range->react[fuzzy_nm][fuzzy_nb] = NB_R;
    range->react[fuzzy_nm][fuzzy_nm] = NB_R;
    range->react[fuzzy_nm][fuzzy_ns] = NM_R;
    range->react[fuzzy_nm][fuzzy_z]  = NM_R;
    range->react[fuzzy_nm][fuzzy_ps] = NS_R;
    range->react[fuzzy_nm][fuzzy_pm] = 0;
    range->react[fuzzy_nm][fuzzy_pb] = PS_R;

    range->react[fuzzy_ns][fuzzy_nb] = NB_R;
    range->react[fuzzy_ns][fuzzy_nm] = NM_R;
    range->react[fuzzy_ns][fuzzy_ns] = NM_R;
    range->react[fuzzy_ns][fuzzy_z]  = NS_R;
    range->react[fuzzy_ns][fuzzy_ps] = 0;
    range->react[fuzzy_ns][fuzzy_pm] = PS_R;
    range->react[fuzzy_ns][fuzzy_pb] = PM_R;

    range->react[fuzzy_z][fuzzy_nb] = NM_R;
    range->react[fuzzy_z][fuzzy_nm] = NM_R;
    range->react[fuzzy_z][fuzzy_ns] = NS_R;
    range->react[fuzzy_z][fuzzy_z]  = 0;
    range->react[fuzzy_z][fuzzy_ps] = PS_R;
    range->react[fuzzy_z][fuzzy_pm] = PM_R;
    range->react[fuzzy_z][fuzzy_pb] = PM_R;

    range->react[fuzzy_ps][fuzzy_nb] = NM_R;
    range->react[fuzzy_ps][fuzzy_nm] = NS_R;
    range->react[fuzzy_ps][fuzzy_ns] = 0;
    range->react[fuzzy_ps][fuzzy_z]  = PS_R;
    range->react[fuzzy_ps][fuzzy_ps] = PS_R;
    range->react[fuzzy_ps][fuzzy_pm] = PM_R;
    range->react[fuzzy_ps][fuzzy_pb] = PB_R;

    range->react[fuzzy_pm][fuzzy_nb] = NS_R;
    range->react[fuzzy_pm][fuzzy_nm] = 0;
    range->react[fuzzy_pm][fuzzy_ns] = PS_R;
    range->react[fuzzy_pm][fuzzy_z]  = PM_R;
    range->react[fuzzy_pm][fuzzy_ps] = PM_R;
    range->react[fuzzy_pm][fuzzy_pm] = PB_R;
    range->react[fuzzy_pm][fuzzy_pb] = PB_R;

    range->react[fuzzy_pb][fuzzy_nb] = 0;
    range->react[fuzzy_pb][fuzzy_nm] = PS_R;
    range->react[fuzzy_pb][fuzzy_ns] = PM_R;
    range->react[fuzzy_pb][fuzzy_z]  = PB_R;
    range->react[fuzzy_pb][fuzzy_ps] = PB_R;
    range->react[fuzzy_pb][fuzzy_pm] = PB_R;
    range->react[fuzzy_pb][fuzzy_pb] = PB_R;
}

